/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/containers/CDynamicGrid.h>
#include <mrpt/containers/NonCopiableData.h>
#include <mrpt/core/safe_pointers.h>
#include <mrpt/img/CImage.h>
#include <mrpt/maps/CLogOddsGridMap2D.h>
#include <mrpt/maps/CLogOddsGridMapLUT.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/maps/NearestNeighborsCapable.h>
#include <mrpt/maps/OccupancyGridCellType.h>
#include <mrpt/obs/CObservation2DRangeScanWithUncertainty.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/tfest/TMatchingPair.h>
#include <mrpt/typemeta/TEnumType.h>

#include <atomic>
#include <mutex>

namespace mrpt::maps
{
/** A class for storing an occupancy grid map.
 *  COccupancyGridMap2D is a class for storing a metric map
 *   representation in the form of a probabilistic occupancy
 *   grid map: value of 0 means certainly occupied, 1 means
 *   a certainly empty cell. Initially 0.5 means uncertainty.
 *
 * The cells keep the log-odd representation of probabilities instead of the
 *probabilities themselves.
 *  More details can be found at https://www.mrpt.org/Occupancy_Grids
 *
 * The algorithm for updating the grid from a laser scanner can optionally take
 *into account the progressive widening of the beams, as
 *   described in [this page](http://www.mrpt.org/Occupancy_Grids)
 *
 *   Some implemented methods are:
 *		- Update of individual cells
 *		- Insertion of observations
 *		- Voronoi diagram and critical points (\a buildVoronoiDiagram)
 *		- Saving and loading from/to a bitmap
 *		- Laser scans simulation for the map contents
 *		- Entropy and information methods (See computeEntropy)
 *
 * \ingroup mrpt_maps_grp
 **/
class COccupancyGridMap2D :
    public CMetricMap,
    public CLogOddsGridMap2D<OccGridCellTraits::cellType>,
    public mrpt::maps::NearestNeighborsCapable
{
  DEFINE_SERIALIZABLE(COccupancyGridMap2D, mrpt::maps)
 public:
  /** The type of the map cells: */
  using cellType = OccGridCellTraits::cellType;
  using cellTypeUnsigned = OccGridCellTraits::cellTypeUnsigned;

  /** Discrete to float conversion factors: The min/max values of the integer
   * cell type, eg.[0,255] or [0,65535] */
  static constexpr cellType OCCGRID_CELLTYPE_MIN = CLogOddsGridMap2D<cellType>::CELLTYPE_MIN;
  static constexpr cellType OCCGRID_CELLTYPE_MAX = CLogOddsGridMap2D<cellType>::CELLTYPE_MAX;

  /** (Default:1.0) Can be set to <1 if a more fine raytracing is needed in
   * sonarSimulator() and laserScanSimulator(), or >1 to speed it up. */
  static double RAYTRACE_STEP_SIZE_IN_CELL_UNITS;

 protected:
  friend class CMultiMetricMap;
  friend class CMultiMetricMapPDF;

  /** Frees the dynamic memory buffers of map. */
  void freeMap();
  /** Lookup tables for log-odds */
  static CLogOddsGridMapLUT<cellType>& get_logodd_lut();

  /** Store of cell occupancy values. Order: row by row, from left to right */
  std::vector<cellType> m_map;
  /** The size of the grid in cells */
  uint32_t m_size_x = 0, m_size_y = 0;
  /** The limits of the grid in "units" (meters) */
  float m_xMin = 0, m_xMax = 0, m_yMin = 0, m_yMax = 0;
  /** Cell size, i.e. resolution of the grid map. */
  float m_resolution = 0;

  /** Auxiliary variables to speed up the computation of observation
   * likelihood values for LF method among others, at a high cost in memory
   * (see TLikelihoodOptions::enableLikelihoodCache). */
  mutable mrpt::containers::NonCopiableData<std::vector<std::atomic<double>>>
      m_precomputedLikelihood;
  mutable bool m_likelihoodCacheOutDated{true};
  mutable mrpt::containers::NonCopiableData<std::mutex> m_precomputedLikelihood_mtx;

  /** Used for Voronoi calculation.Same struct as "map", but contains a "0" if
   * not a basis point. */
  mrpt::containers::CDynamicGrid<uint8_t> m_basis_map;

  /** Used to store the Voronoi diagram.
   *    Contains the distance of each cell to its closer obstacles
   *    in 1/100th distance units (i.e. in centimeters), or 0 if not into the
   * Voronoi diagram  */
  mrpt::containers::CDynamicGrid<uint16_t> m_voronoi_diagram;

  /** True upon construction; used by isEmpty() */
  bool m_is_empty{true};

  /** See base class */
  void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation&) override;

  /** The free-cells threshold used to compute the Voronoi diagram. */
  float voroni_free_threshold{};

  /** Entropy computation internal function: */
  template <typename T>
  static T H(const T p)
  {
    if (p == 0 || p == 1)
      return 0;
    else
      return -p * std::log(p);
  }
  /** Internally used to speed-up entropy calculation */
  static std::vector<float> entropyTable;

  /** Change the contents [0,1] of a cell, given its index */
  inline void setCell_nocheck(int x, int y, float value) { m_map[x + y * m_size_x] = p2l(value); }

  /** Read the real valued [0,1] contents of a cell, given its index */
  inline float getCell_nocheck(int x, int y) const { return l2p(m_map[x + y * m_size_x]); }
  /** Changes a cell by its absolute index (Do not use it normally) */
  inline void setRawCell(unsigned int cellIndex, cellType b)
  {
    if (cellIndex < m_size_x * m_size_y) m_map[cellIndex] = b;
  }

  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood" (This method is the Range-Scan Likelihood
   * Consensus for gridmaps, see the ICRA2007 paper by Blanco et al.)  */
  double computeObservationLikelihood_Consensus(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood". TODO: This method is described in....  */
  double computeObservationLikelihood_ConsensusOWA(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood"  */
  double computeObservationLikelihood_CellsDifference(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood" */
  double computeObservationLikelihood_MI(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood" */
  double computeObservationLikelihood_rayTracing(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood".*/
  double computeObservationLikelihood_likelihoodField_Thrun(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;
  /** One of the methods that can be selected for implementing
   * "computeObservationLikelihood". */
  double computeObservationLikelihood_likelihoodField_II(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose2D& takenFrom) const;

  /** Clear the map: It set all cells to their default occupancy value (0.5),
   * without changing the resolution (the grid extension is reset to the
   * default values). */
  void internal_clear() override;

  /** Insert the observation information into this map.
   *
   * \param obs The observation
   * \param robotPose The 3D pose of the robot mobile base in the map
   * reference system, or nullptr (default) if you want to use
   * CPose2D(0,0,deg)
   *
   *  After successful execution, "lastObservationInsertionInfo" is updated.
   *
   * \sa insertionOptions, CObservation::insertObservationInto
   */
  bool internal_insertObservation(
      const mrpt::obs::CObservation& obs,
      const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt) override;

 public:
  /** Read-only access to the raw cell contents (cells are in log-odd units)
   */
  const std::vector<cellType>& getRawMap() const { return this->m_map; }
  /** Performs the Bayesian fusion of a new observation of a cell  \sa
   * updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
  void updateCell(int x, int y, float v);

  /** An internal structure for storing data related to counting the new
   * information apported by some observation */
  struct TUpdateCellsInfoChangeOnly
  {
    TUpdateCellsInfoChangeOnly() = default;

    /** If set to false (default), this struct is not used. Set to true only
     * when measuring the info of an observation. */
    bool enabled{false};
    /** The cummulative change in Information: This is updated only from the
     * "updateCell" method. */
    double I_change{0};
    /** The cummulative updated cells count: This is updated only from the
     * "updateCell" method. */
    int cellsUpdated{0};
    /** In this mode, some laser rays can be skips to speep-up */
    int laserRaysSkip{1};
  };

  mutable TUpdateCellsInfoChangeOnly updateInfoChangeOnly;

  /** Fills all the cells with a default value. */
  void fill(float default_value = 0.5f);

  /** Constructor */
  COccupancyGridMap2D(
      float min_x = -20.0f,
      float max_x = 20.0f,
      float min_y = -20.0f,
      float max_y = 20.0f,
      float resolution = 0.05f);

  /** Change the size of gridmap, erasing all its previous contents.
   * \param x_min The "x" coordinates of left most side of grid.
   * \param x_max The "x" coordinates of right most side of grid.
   * \param y_min The "y" coordinates of top most side of grid.
   * \param y_max The "y" coordinates of bottom most side of grid.
   * \param resolution The new size of cells.
   * \param default_value The value of cells, typically 0.5.
   * \sa ResizeGrid
   */
  void setSize(
      float x_min,
      float x_max,
      float y_min,
      float y_max,
      float resolution,
      float default_value = 0.5f);

  /** Change the size of gridmap, maintaining previous contents.
   * \param new_x_min The "x" coordinates of new left most side of grid.
   * \param new_x_max The "x" coordinates of new right most side of grid.
   * \param new_y_min The "y" coordinates of new top most side of grid.
   * \param new_y_max The "y" coordinates of new bottom most side of grid.
   * \param new_cells_default_value The value of the new cells, typically 0.5.
   * \param additionalMargin If set to true (default), an additional margin of
   * a few meters will be added to the grid, ONLY if the new coordinates are
   * larger than current ones.
   * \sa setSize
   */
  void resizeGrid(
      float new_x_min,
      float new_x_max,
      float new_y_min,
      float new_y_max,
      float new_cells_default_value = 0.5f,
      bool additionalMargin = true) noexcept;

  /** Returns the area of the gridmap, in square meters */
  inline double getArea() const { return m_size_x * m_size_y * mrpt::square(m_resolution); }

  /** Returns the horizontal size of grid map in cells count */
  inline unsigned int getSizeX() const { return m_size_x; }
  /** Returns the vertical size of grid map in cells count */
  inline unsigned int getSizeY() const { return m_size_y; }
  /** Returns the "x" coordinate of left side of grid map */
  inline float getXMin() const { return m_xMin; }
  /** Returns the "x" coordinate of right side of grid map */
  inline float getXMax() const { return m_xMax; }
  /** Returns the "y" coordinate of top side of grid map */
  inline float getYMin() const { return m_yMin; }
  /** Returns the "y" coordinate of bottom side of grid map */
  inline float getYMax() const { return m_yMax; }
  /** Returns the resolution of the grid map */
  inline float getResolution() const { return m_resolution; }
  /** Transform a coordinate value into a cell index */
  inline int x2idx(float x) const { return static_cast<int>((x - m_xMin) / m_resolution); }
  inline int y2idx(float y) const { return static_cast<int>((y - m_yMin) / m_resolution); }

  inline int x2idx(double x) const { return static_cast<int>((x - m_xMin) / m_resolution); }
  inline int y2idx(double y) const { return static_cast<int>((y - m_yMin) / m_resolution); }

  /** Transform a cell index into a coordinate value */
  inline float idx2x(size_t cx) const { return m_xMin + (cx + 0.5f) * m_resolution; }
  inline float idx2y(size_t cy) const { return m_yMin + (cy + 0.5f) * m_resolution; }

  /** Transform a coordinate value into a cell index, using a different "x_min"
   * value */
  inline int x2idx(float x, float xmin) const
  {
    return static_cast<int>((x - xmin) / m_resolution);
  }
  inline int y2idx(float y, float ymin) const
  {
    return static_cast<int>((y - ymin) / m_resolution);
  }

  mrpt::math::TBoundingBoxf boundingBox() const override
  {
    return {
        {m_xMin, m_yMin, insertionOptions.mapAltitude},
        {m_xMax, m_yMax, insertionOptions.mapAltitude}
    };
  }

  /** Scales an integer representation of the log-odd into a real valued
   * probability in [0,1], using p=exp(l)/(1+exp(l))  */
  static inline float l2p(const cellType l) { return get_logodd_lut().l2p(l); }
  /** Scales an integer representation of the log-odd into a linear scale
   * [0,255], using p=exp(l)/(1+exp(l)) */
  static inline uint8_t l2p_255(const cellType l) { return get_logodd_lut().l2p_255(l); }
  /** Scales a real valued probability in [0,1] to an integer representation
   * of: log(p)-log(1-p)  in the valid range of cellType */
  static inline cellType p2l(const float p) { return get_logodd_lut().p2l(p); }
  /** Change the contents [0,1] of a cell, given its index */
  inline void setCell(int x, int y, float value)
  {
    // The x> comparison implicitly holds if x<0
    if (static_cast<unsigned int>(x) >= m_size_x || static_cast<unsigned int>(y) >= m_size_y)
      return;
    else
      m_map[x + y * m_size_x] = p2l(value);
  }

  /** Read the real valued [0,1] contents of a cell, given its index */
  inline float getCell(int x, int y) const
  {
    // The x> comparison implicitly holds if x<0
    if (static_cast<unsigned int>(x) >= m_size_x || static_cast<unsigned int>(y) >= m_size_y)
      return 0.5f;
    else
      return l2p(m_map[x + y * m_size_x]);
  }

  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently,
   * do not use it normally */
  inline cellType* getRow(int cy)
  {
    if (cy < 0 || static_cast<unsigned int>(cy) >= m_size_y)
      return nullptr;
    else
      return &m_map[0 + cy * m_size_x];
  }

  /** Access to a "row": mainly used for drawing grid as a bitmap efficiently,
   * do not use it normally */
  inline const cellType* getRow(int cy) const
  {
    if (cy < 0 || static_cast<unsigned int>(cy) >= m_size_y)
      return nullptr;
    else
      return &m_map[0 + cy * m_size_x];
  }

  /** Change the contents [0,1] of a cell, given its coordinates */
  inline void setPos(float x, float y, float value) { setCell(x2idx(x), y2idx(y), value); }

  /** Read the real valued [0,1] contents of a cell, given its coordinates */
  inline float getPos(float x, float y) const { return getCell(x2idx(x), y2idx(y)); }

  /** Returns "true" if cell is "static", i.e.if its occupancy is below a
   * given threshold */
  inline bool isStaticPos(float x, float y, float threshold = 0.7f) const
  {
    return isStaticCell(x2idx(x), y2idx(y), threshold);
  }
  inline bool isStaticCell(int cx, int cy, float threshold = 0.7f) const
  {
    return (getCell(cx, cy) <= threshold);
  }

  /** Change a cell in the "basis" maps.Used for Voronoi calculation */
  inline void setBasisCell(int x, int y, uint8_t value)
  {
    uint8_t* cell = m_basis_map.cellByIndex(x, y);
#ifdef _DEBUG
    ASSERT_GE_(x, 0);
    ASSERT_GE_(y, 0);
    ASSERT_LE_(x, int(m_basis_map.getSizeX()));
    ASSERT_LE_(y, int(m_basis_map.getSizeY()));
#endif
    *cell = value;
  }

  /** Reads a cell in the "basis" maps.Used for Voronoi calculation */
  inline unsigned char getBasisCell(int x, int y) const
  {
    const uint8_t* cell = m_basis_map.cellByIndex(x, y);
#ifdef _DEBUG
    ASSERT_GE_(x, 0);
    ASSERT_GE_(y, 0);
    ASSERT_LE_(x, int(m_basis_map.getSizeX()));
    ASSERT_LE_(y, int(m_basis_map.getSizeY()));
#endif
    return *cell;
  }

  /** copy the gridmap contents, but not all the options, from another map
   * instance */
  void copyMapContentFrom(const COccupancyGridMap2D& otherMap);

  /** Used for returning entropy related information \sa computeEntropy */
  struct TEntropyInfo
  {
    /** The target variable for absolute entropy, computed
     * as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)*ln(p(x,y))
     * -(1-p(x,y))*ln(1-p(x,y)) }</center><br><br> */
    double H{0};
    /** The target variable for absolute "information", defining I(x) = 1 -
     * H(x) */
    double I{0};
    /** The target variable for mean entropy, defined as entropy per cell:
     * mean_H(map) = H(map) / (cells) */
    double mean_H{0};
    /** The target variable for mean information, defined as information per
     * cell: mean_I(map) = I(map) / (cells) */
    double mean_I{0};
    /** The target variable for the area of cells with information, i.e.
     * p(x)!=0.5 */
    double effectiveMappedArea{0};
    /** The mapped area in cells. */
    unsigned long effectiveMappedCells{0};
  };

  /** With this struct options are provided to the observation insertion
   * process.
   * \sa CObservation::insertIntoGridMap */
  class TInsertionOptions : public mrpt::config::CLoadableOptions
  {
   public:
    TInsertionOptions() = default;

    /** This method load the options from a ".ini" file.
     *   Only those parameters found in the given "section" and having
     *   the same name that the variable are loaded. Those not found in
     *   the file will stay with their previous values (usually the default
     *   values loaded at initialization). An example of an ".ini" file:
     *  \code
     *  [section]
     *	resolution=0.10		; blah blah...
     *	modeSelection=1		; 0=blah, 1=blah,...
     *  \endcode
     */
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;                 // See base docs
    void dumpToTextStream(std::ostream& out) const override;  // See base docs

    /** The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for
     * they to be inserted in this map! */
    float mapAltitude{0};
    /** The parameter "mapAltitude" has effect while inserting observations
     * in the grid only if this is true. */
    bool useMapAltitude{false};
    /** The largest distance at which cells will be updated (Default 15
     * meters) */
    float maxDistanceInsertion{15.0f};
    /** A value in the range [0.5,1] used for updating cell with a bayesian
     * approach (default 0.8) */
    float maxOccupancyUpdateCertainty{0.65f};
    /** A value in the range [0.5,1] for updating a free cell. (default=0
     * means use the same than maxOccupancyUpdateCertainty) */
    float maxFreenessUpdateCertainty{.0f};
    /** Like maxFreenessUpdateCertainty, but for invalid ranges (no echo).
     * (default=0 means same than maxOccupancyUpdateCertainty)*/
    float maxFreenessInvalidRanges{.0f};
    /** If set to true (default), invalid range values (no echo rays) as
     * consider as free space until "maxOccupancyUpdateCertainty", but ONLY
     * when the previous and next rays are also an invalid ray. */
    bool considerInvalidRangesAsFreeSpace{true};
    /** Specify the decimation of the range scan (default=1 : take all the
     * range values!) */
    uint16_t decimation{1};
    /** The tolerance in rads in pitch & roll for a laser scan to be
     * considered horizontal, then processed by calls to this class
     * (default=0). */
    float horizontalTolerance{mrpt::DEG2RAD(0.05f)};
    /** Gaussian sigma of the filter used in getAsImageFiltered (for
     * features detection) (Default=1) (0:Disabled)  */
    float CFD_features_gaussian_size{1};
    /** Size of the Median filter used in getAsImageFiltered (for features
     * detection) (Default=3) (0:Disabled) */
    float CFD_features_median_size{3};
    /** Enabled: Rays widen with distance to approximate the real behavior
     * of lasers, disabled: insert rays as simple lines (Default=false) */
    bool wideningBeamsWithDistance{false};
  };

  /** With this struct options are provided to the observation insertion
   * process \sa CObservation::insertIntoGridMap */
  TInsertionOptions insertionOptions;

  /** The type for selecting a likelihood computation method */
  enum TLikelihoodMethod
  {
    lmMeanInformation = 0,
    lmRayTracing,
    lmConsensus,
    lmCellsDifference,
    lmLikelihoodField_Thrun,
    lmLikelihoodField_II,
    lmConsensusOWA
    // Remember: Update TEnumType below if new values are added here!
  };

  /** With this struct options are provided to the observation likelihood
   * computation process */
  class TLikelihoodOptions : public mrpt::config::CLoadableOptions
  {
   public:
    /** Initialization of default parameters */
    TLikelihoodOptions();

    /** This method load the options from a ".ini" file.
     *   Only those parameters found in the given "section" and having
     *   the same name that the variable are loaded. Those not found in
     *   the file will stay with their previous values (usually the default
     *   values loaded at initialization). An example of an ".ini" file:
     *  \code
     *  [section]
     *	resolution=0.10		; blah blah...
     *	modeSelection=1		; 0=blah, 1=blah,...
     *  \endcode
     */
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;                 // See base docs
    void dumpToTextStream(std::ostream& out) const override;  // See base docs

    /** The selected method to compute an observation likelihood */
    TLikelihoodMethod likelihoodMethod{lmLikelihoodField_Thrun};
    /** [LikelihoodField] The laser range "sigma" used in computations;
     * Default value = 0.35 */
    float LF_stdHit{0.35f};
    /** [LikelihoodField] */
    float LF_zHit{0.95f}, LF_zRandom{0.05f};
    /** [LikelihoodField] The max. range of the sensor (Default= 81 m) */
    float LF_maxRange{81.0f};
    /** [LikelihoodField] The decimation of the points in a scan, default=1
     * == no decimation */
    uint32_t LF_decimation{5};
    /** [LikelihoodField] The max. distance for searching correspondences
     * around each sensed point */
    float LF_maxCorrsDistance{0.3f};
    /** [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of
     * `exp(dist^2/std^2)` */
    bool LF_useSquareDist{false};
    /** [LikelihoodField] Set this to "true" ot use an alternative method,
     * where the likelihood of the whole range scan is computed by
     * "averaging" of individual ranges, instead of by the "product". */
    bool LF_alternateAverageMethod{false};
    /** [MI] The exponent in the MI likelihood computation. Default value =
     * 5 */
    float MI_exponent{2.5f};
    /** [MI] The scan rays decimation: at every N rays, one will be used to
     * compute the MI */
    uint32_t MI_skip_rays{10};
    /** [MI] The ratio for the max. distance used in the MI computation and
     * in the insertion of scans, e.g. if set to 2.0 the MI will use twice
     * the distance that the update distance. */
    float MI_ratio_max_distance{1.5f};
    /** [rayTracing] If true (default), the rayTracing method will ignore
     * measured ranges shorter than the simulated ones. */
    bool rayTracing_useDistanceFilter{true};
    /** [rayTracing] One out of "rayTracing_decimation" rays will be
     * simulated and compared only: set to 1 to use all the sensed ranges.
     */
    int32_t rayTracing_decimation{10};
    /** [rayTracing] The laser range sigma. */
    float rayTracing_stdHit{1.0f};
    /** [Consensus] The down-sample ratio of ranges (default=1, consider all
     * the ranges) */
    int32_t consensus_takeEachRange{1};
    /** [Consensus] The power factor for the likelihood (default=5) */
    float consensus_pow{5};
    /** [OWA] The sequence of weights to be multiplied to of the ordered
     * list of likelihood values (first one is the largest); the size of
     * this vector determines the number of highest likelihood values to
     * fuse. */
    std::vector<float> OWA_weights;

    /** Enables the usage of a cache of likelihood values (for LF methods),
     * if set to true (default=false). */
    bool enableLikelihoodCache{true};
  } likelihoodOptions;

  /** Auxiliary private class. */
  using TPairLikelihoodIndex = std::pair<double, mrpt::math::TPoint2D>;

  /** Some members of this struct will contain intermediate or output data
   * after calling "computeObservationLikelihood" for some likelihood
   * functions */
  struct TLikelihoodOutput
  {
   public:
    /** [OWA method] This will contain the ascending-ordered list of
     * pairs:(likelihood values, 2D point in map coordinates). */
    std::vector<TPairLikelihoodIndex> OWA_pairList;

    /** [OWA method] This will contain the ascending-ordered list of
     * likelihood values for individual range measurements in the scan. */
    std::vector<double> OWA_individualLikValues;
  };

  mutable TLikelihoodOutput likelihoodOutputs;

  /** Performs a downsampling of the gridmap, by a given factor:
   * resolution/=ratio */
  void subSample(int downRatio);

  /** Computes the entropy and related values of this grid map.
   *  The entropy is computed as the summed entropy of each cell, taking them
   * as discrete random variables following a Bernoulli distribution:
   * \param info The output information is returned here */
  void computeEntropy(TEntropyInfo& info) const;

  /** @name Voronoi methods
    @{ */

  /** Build the Voronoi diagram of the grid map.
   * \param threshold The threshold for binarizing the map.
   * \param robot_size Size in "units" (meters) of robot, approx.
   * \param x1 Left coordinate of area to be computed. Default, entire map.
   * \param x2 Right coordinate of area to be computed. Default, entire map.
   * \param y1 Top coordinate of area to be computed. Default, entire map.
   * \param y2 Bottom coordinate of area to be computed. Default, entire map.
   * \sa findCriticalPoints
   */
  void buildVoronoiDiagram(
      float threshold, float robot_size, int x1 = 0, int x2 = 0, int y1 = 0, int y2 = 0);

  /** Reads a the clearance of a cell (in centimeters), after building the
   * Voronoi diagram with \a buildVoronoiDiagram */
  inline uint16_t getVoroniClearance(int cx, int cy) const
  {
#ifdef _DEBUG
    ASSERT_GE_(cx, 0);
    ASSERT_GE_(cy, 0);
    ASSERT_LE_(cx, int(m_voronoi_diagram.getSizeX()));
    ASSERT_LE_(cy, int(m_voronoi_diagram.getSizeY()));
#endif
    const uint16_t* cell = m_voronoi_diagram.cellByIndex(cx, cy);
    return *cell;
  }

 protected:
  /** Used to set the clearance of a cell, while building the Voronoi diagram.
   */
  inline void setVoroniClearance(int cx, int cy, uint16_t dist)
  {
    uint16_t* cell = m_voronoi_diagram.cellByIndex(cx, cy);
#ifdef _DEBUG
    ASSERT_GE_(cx, 0);
    ASSERT_GE_(cy, 0);
    ASSERT_LE_(cx, int(m_voronoi_diagram.getSizeX()));
    ASSERT_LE_(cy, int(m_voronoi_diagram.getSizeY()));
#endif
    *cell = dist;
  }

 public:
  /** Return the auxiliary "basis" map built while building the Voronoi
   * diagram \sa buildVoronoiDiagram */
  inline const mrpt::containers::CDynamicGrid<uint8_t>& getBasisMap() const { return m_basis_map; }

  /** Return the Voronoi diagram; each cell contains the distance to its
   * closer obstacle, or 0 if not part of the Voronoi diagram \sa
   * buildVoronoiDiagram */
  inline const mrpt::containers::CDynamicGrid<uint16_t>& getVoronoiDiagram() const
  {
    return m_voronoi_diagram;
  }

  /** Builds a list with the critical points from Voronoi diagram, which must
   *    must be built before calling this method.
   * \param filter_distance The minimum distance between two critical points.
   * \sa buildVoronoiDiagram
   */
  void findCriticalPoints(float filter_distance);

  /** @} */  // End of Voronoi methods

  /** Compute the clearance of a given cell, and returns its two first
   *   basis (closest obstacle) points.Used to build Voronoi and critical
   * points.
   * \return The clearance of the cell, in 1/100 of "cell".
   * \param cx The cell index
   * \param cy The cell index
   * \param basis_x Target buffer for coordinates of basis, having a size of
   * two "ints".
   * \param basis_y Target buffer for coordinates of basis, having a size of
   * two "ints".
   * \param nBasis The number of found basis: Can be 0,1 or 2.
   * \param GetContourPoint If "true" the basis are not returned, but the
   * closest free cells.Default at false.
   * \sa Build_VoronoiDiagram
   */
  int computeClearance(
      int cx, int cy, int* basis_x, int* basis_y, int* nBasis, bool GetContourPoint = false) const;

  /** An alternative method for computing the clearance of a given location
   * (in meters).
   *  \return The clearance (distance to closest OCCUPIED cell), in meters.
   */
  float computeClearance(float x, float y, float maxSearchDistance) const;

  /** Compute the 'cost' of traversing a segment of the map according to the
   * occupancy of traversed cells.
   *  \return This returns '1-mean(traversed cells occupancy)', i.e. 0.5 for
   * unknown cells, 1 for a free path.
   */
  float computePathCost(float x1, float y1, float x2, float y2) const;

  /** \name Sensor simulators
    @{ */

  /** Simulates a laser range scan into the current grid map.
   *   The simulated scan is stored in a CObservation2DRangeScan object, which
   *is also used
   *    to pass some parameters: all previously stored characteristics (as
   *aperture,...) are
   *	  taken into account for simulation. Only a few more parameters are
   *needed. Additive gaussian noise can be optionally added to the simulated
   *scan.
   * \param inout_Scan [IN/OUT] This must be filled with desired parameters
   *before calling, and will contain the scan samples on return.
   * \param robotPose [IN] The robot pose in this map coordinates. Recall that
   *sensor pose relative to this robot pose must be specified in the
   *observation object.
   * \param threshold [IN] The minimum occupancy threshold to consider a cell
   *to be occupied (Default: 0.5f)
   * \param N [IN] The count of range scan "rays", by default to 361.
   * \param noiseStd [IN] The standard deviation of measurement noise. If not
   *desired, set to 0.
   * \param decimation [IN] The rays that will be simulated are at indexes: 0,
   *D, 2D, 3D, ... Default is D=1
   * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added
   *to the angles at which ranges are measured (in radians).
   *
   * \sa laserScanSimulatorWithUncertainty(), sonarSimulator(),
   *COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
   */
  void laserScanSimulator(
      mrpt::obs::CObservation2DRangeScan& inout_Scan,
      const mrpt::poses::CPose2D& robotPose,
      float threshold = 0.6f,
      size_t N = 361,
      float noiseStd = 0,
      unsigned int decimation = 1,
      float angleNoiseStd = mrpt::DEG2RAD(.0)) const;

  /** Simulates the observations of a sonar rig into the current grid map.
   *   The simulated ranges are stored in a CObservationRange object, which is
   * also used
   *    to pass in some needed parameters, as the poses of the sonar sensors
   * onto the mobile robot.
   * \param inout_observation [IN/OUT] This must be filled with desired
   * parameters before calling, and will contain the simulated ranges on
   * return.
   * \param robotPose [IN] The robot pose in this map coordinates. Recall that
   * sensor pose relative to this robot pose must be specified in the
   * observation object.
   * \param threshold [IN] The minimum occupancy threshold to consider a cell
   * to be occupied (Default: 0.5f)
   * \param rangeNoiseStd [IN] The standard deviation of measurement noise. If
   * not desired, set to 0.
   * \param angleNoiseStd [IN] The sigma of an optional Gaussian noise added
   * to the angles at which ranges are measured (in radians).
   *
   * \sa laserScanSimulator(),
   * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
   */
  void sonarSimulator(
      mrpt::obs::CObservationRange& inout_observation,
      const mrpt::poses::CPose2D& robotPose,
      float threshold = 0.5f,
      float rangeNoiseStd = 0.f,
      float angleNoiseStd = mrpt::DEG2RAD(0.f)) const;

  /** Simulate just one "ray" in the grid map. This method is used internally
   * to sonarSimulator and laserScanSimulator. \sa
   * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS */
  void simulateScanRay(
      const double x,
      const double y,
      const double angle_direction,
      float& out_range,
      bool& out_valid,
      const double max_range_meters,
      const float threshold_free = 0.4f,
      const double noiseStd = .0,
      const double angleNoiseStd = .0) const;

  /** Methods for TLaserSimulUncertaintyParams in
   * laserScanSimulatorWithUncertainty() */
  enum TLaserSimulUncertaintyMethod
  {
    /** Performs an unscented transform */
    sumUnscented = 0,
    /** Montecarlo-based estimation */
    sumMonteCarlo
  };

  /** Input params for laserScanSimulatorWithUncertainty() */
  struct TLaserSimulUncertaintyParams
  {
    /** (Default: sumMonteCarlo) Select the method to do the uncertainty
     * propagation */
    TLaserSimulUncertaintyMethod method{sumUnscented};
    /** @name Parameters for each uncertainty method
      @{ */
    /** [sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0,
     * betta=2.0 */
    double UT_alpha{0.99}, UT_kappa{.0}, UT_beta{2.0};
    /** [sumMonteCarlo] MonteCarlo parameter: number of samples (Default:
     * 10) */
    size_t MC_samples{10};
    /** @} */

    /** @name Generic parameters for all methods
      @{ */
    /** The robot pose Gaussian, in map coordinates. Recall that sensor pose
     * relative to this robot pose must be specified in the observation
     * object */
    mrpt::poses::CPosePDFGaussian robotPose;
    /** (Default: M_PI) The "aperture" or field-of-view of the range finder,
     * in radians (typically M_PI = 180 degrees). */
    float aperture{M_PIf};
    /** (Default: true) The scanning direction: true=counterclockwise;
     * false=clockwise */
    bool rightToLeft{true};
    /** (Default: 80) The maximum range allowed by the device, in meters
     * (e.g. 80m, 50m,...) */
    float maxRange{80.f};
    /** (Default: at origin) The 6D pose of the sensor on the robot at the
     * moment of starting the scan. */
    mrpt::poses::CPose3D sensorPose;
    size_t nRays{361};
    /** (Default: 0) The standard deviation of measurement noise. If not
     * desired, set to 0 */
    float rangeNoiseStd{.0f};
    /** (Default: 0) The sigma of an optional Gaussian noise added to the
     * angles at which ranges are measured (in radians) */
    float angleNoiseStd{.0f};
    /** (Default: 1) The rays that will be simulated are at indexes: 0, D,
     * 2D, 3D,... */
    unsigned int decimation{1};
    /** (Default: 0.6f) The minimum occupancy threshold to consider a cell
     * to be occupied */
    float threshold{.6f};
    /** @} */

    TLaserSimulUncertaintyParams();
  };

  /** Output params for laserScanSimulatorWithUncertainty() */
  struct TLaserSimulUncertaintyResult
  {
    /** The scan + its uncertainty */
    mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert;

    TLaserSimulUncertaintyResult();
  };

  /** Like laserScanSimulatorWithUncertainty() (see it for a discussion of
   * most parameters) but taking into account
   *  the robot pose uncertainty and generating a vector of predicted
   * variances for each ray.
   *  Range uncertainty includes both, sensor noise and large non-linear
   * effects caused by borders and discontinuities in the environment
   *  as seen from different robot poses.
   *
   * \param in_params [IN] Input settings. See TLaserSimulUncertaintyParams
   * \param in_params [OUT] Output range + uncertainty.
   *
   * \sa laserScanSimulator(),
   * COccupancyGridMap2D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS
   */
  void laserScanSimulatorWithUncertainty(
      const TLaserSimulUncertaintyParams& in_params,
      TLaserSimulUncertaintyResult& out_results) const;

  /** @} */

  /** Computes the likelihood [0,1] of a set of points, given the current grid
   * map as reference.
   * \param pm The points map
   * \param relativePose The relative pose of the points map in this map's
   * coordinates, or nullptr for (0,0,0).
   *  See "likelihoodOptions" for configuration parameters.
   */
  double computeLikelihoodField_Thrun(
      const CPointsMap* pm, const mrpt::poses::CPose2D* relativePose = nullptr) const;

  /** Computes the likelihood [0,1] of a set of points, given the current grid
   * map as reference.
   * \param pm The points map
   * \param relativePose The relative pose of the points map in this map's
   * coordinates, or nullptr for (0,0,0).
   *  See "likelihoodOptions" for configuration parameters.
   */
  double computeLikelihoodField_II(
      const CPointsMap* pm, const mrpt::poses::CPose2D* relativePose = nullptr) const;

  /** Saves the gridmap as a graphical file (BMP,PNG,...).
   * The format will be derived from the file extension (see
   * CImage::saveToFile )
   * \return False on any error.
   */
  bool saveAsBitmapFile(const std::string& file) const;

  /** Saves a composite image with two gridmaps and lines representing a set
   * of correspondences between them.
   * \return False on any error.
   */
  static bool saveAsBitmapTwoMapsWithCorrespondences(
      const std::string& fileName,
      const COccupancyGridMap2D* m1,
      const COccupancyGridMap2D* m2,
      const mrpt::tfest::TMatchingPairList& corrs);

  /** Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel
   * is 1 cell, and with an overlay of landmarks.
   * \note The template parameter CLANDMARKSMAP is assumed to be
   * mrpt::maps::CLandmarksMap normally.
   * \return False on any error.
   */
  template <class CLANDMARKSMAP>
  bool saveAsBitmapFileWithLandmarks(
      const std::string& file,
      const CLANDMARKSMAP* landmarks,
      bool addTextLabels = false,
      const mrpt::img::TColor& marks_color = mrpt::img::TColor(0, 0, 255)) const
  {
    MRPT_START
    mrpt::img::CImage img;
    getAsImageFiltered(img, false, true);  // in RGB
    const bool topleft = img.isOriginTopLeft();
    for (unsigned int i = 0; i < landmarks->landmarks.size(); i++)
    {
      const typename CLANDMARKSMAP::landmark_type* lm = landmarks->landmarks.get(i);
      int px = x2idx(lm->pose_mean.x);
      int py = topleft ? m_size_y - 1 - y2idx(lm->pose_mean.y) : y2idx(lm->pose_mean.y);
      img.rectangle(px - 7, (py + 7), px + 7, (py - 7), marks_color);
      img.rectangle(px - 6, (py + 6), px + 6, (py - 6), marks_color);
      if (addTextLabels) img.textOut(px, py - 8, format("%u", i), mrpt::img::TColor::black());
    }
    return img.saveToFile(file.c_str());
    MRPT_END
  }

  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell
   * (output image is RGB only if forceRGB is true)
   *  If "tricolor" is true, only three gray levels will appear in the image:
   * gray for unobserved cells, and black/white for occupied/empty cells
   * respectively.
   * \sa getAsImageFiltered
   */
  void getAsImage(
      mrpt::img::CImage& img,
      bool verticalFlip = false,
      bool forceRGB = false,
      bool tricolor = false) const;

  /** Returns the grid as a 8-bit graylevel image, where each pixel is a cell
   * (output image is RGB only if forceRGB is true) - This method filters the
   * image for easy feature detection
   *  If "tricolor" is true, only three gray levels will appear in the image:
   * gray for unobserved cells, and black/white for occupied/empty cells
   * respectively.
   * \sa getAsImage
   */
  void getAsImageFiltered(img::CImage& img, bool verticalFlip = false, bool forceRGB = false) const;

  /** Returns a 3D plane with its texture being the occupancy grid and
   * transparency proportional to "uncertainty" (i.e. a value of 0.5 is fully
   * transparent)
   */
  void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const override;

  /** Get a point cloud with all (border) occupied cells as points */
  void getAsPointCloud(mrpt::maps::CSimplePointsMap& pm, const float occup_threshold = 0.5f) const;

  /** Returns true upon map construction or after calling clear(), the return
   *  changes to false upon successful insertObservation() or any other
   * method to load data in the map.
   */
  bool isEmpty() const override;

  /** Load the gridmap from a image in a file (the format can be any supported
   * by CImage::loadFromFile).
   * \param file The file to be loaded.
   * \param resolution The size of a pixel (cell), in meters. Recall cells are
   * always squared, so just a dimension is needed.
   * \param origin The `x` (0=first, increases <b>left to right</b> on the
   * image) and `y` (0=first, increases <b>BOTTOM upwards</b> on the image)
   * coordinates for the pixel which will be taken at the origin of map
   * coordinates (0,0). (Default=center of the image) \return False on any
   * error. \sa loadFromBitmap
   */
  bool loadFromBitmapFile(
      const std::string& file,
      float resolution,
      const mrpt::math::TPoint2D& origin = mrpt::math::TPoint2D(
          std::numeric_limits<double>::max(), std::numeric_limits<double>::max()));

  /** Load the gridmap from a image in a file (the format can be any supported
   * by CImage::loadFromFile).
   *  See loadFromBitmapFile() for the meaning of parameters */
  bool loadFromBitmap(
      const mrpt::img::CImage& img,
      float resolution,
      const mrpt::math::TPoint2D& origin = mrpt::math::TPoint2D(
          std::numeric_limits<double>::max(), std::numeric_limits<double>::max()));

  /** Loads this gridmap from a .yaml file and an accompanying image file
   *  given in the
   *  [map_server YAML](http://wiki.ros.org/map_server#YAML_format) file
   *  format.
   *
   * \param yamlFilePath Absolute or relative path to the `.yaml` file.
   *
   * \return false on error, true on success.
   * \sa FromROSMapServerYAML()
   */
  bool loadFromROSMapServerYAML(const std::string& yamlFilePath);

  /** Creates a gridmap from a .yaml file and an accompanying image file
   *  given in the
   *  [map_server YAML](http://wiki.ros.org/map_server#YAML_format) file
   *  format.
   *
   * \param yamlFilePath Absolute or relative path to the `.yaml` file.
   *
   * \sa loadFromROSMapServerYAML()
   * \exception std::exception On error loading or parsing the files.
   */
  static COccupancyGridMap2D FromROSMapServerYAML(const std::string& yamlFilePath)
  {
    COccupancyGridMap2D grid;
    if (!grid.loadFromROSMapServerYAML(yamlFilePath))
      THROW_EXCEPTION_FMT("Error loading gridmap from '%s'", yamlFilePath.c_str());
    return grid;
  }

  /** See the base class for more details: In this class it is implemented as
   * correspondences of the passed points map to occupied cells.
   * NOTICE: That the "z" dimension is ignored in the points. Clip the points
   * as appropiated if needed before calling this method.
   *
   * \sa computeMatching3DWith
   */
  void determineMatching2D(
      const mrpt::maps::CMetricMap* otherMap,
      const mrpt::poses::CPose2D& otherMapPose,
      mrpt::tfest::TMatchingPairList& correspondences,
      const TMatchingParams& params,
      TMatchingExtraResults& extraResults) const override;

  /** See docs in base class: in this class this always returns 0 */
  float compute3DMatchingRatio(
      const mrpt::maps::CMetricMap* otherMap,
      const mrpt::poses::CPose3D& otherMapPose,
      const TMatchingRatioParams& params) const override;

  /** This virtual method saves the map to a file "filNamePrefix"+<
   * some_file_extension >, as an image or in any other applicable way (Notice
   * that other methods to save the map may be implemented in classes
   * implementing this virtual interface).  */
  void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const override;

  /** The structure used to store the set of Voronoi diagram
   *    critical points.
   * \sa findCriticalPoints
   */
  struct TCriticalPointsList
  {
    TCriticalPointsList() : x(), y(), clearance(), x_basis1(), y_basis1(), x_basis2(), y_basis2() {}

    /** The coordinates of critical point. */
    std::vector<int> x, y;
    /** The clearance of critical points, in 1/100 of cells. */
    std::vector<int> clearance;
    /** Their two first basis points coordinates. */
    std::vector<int> x_basis1, y_basis1, x_basis2, y_basis2;
  } CriticalPointsList;

  /** Returns a short description of the map. */
  std::string asString() const override
  {
    return mrpt::format(
        "2D gridmap, extending from (%f,%f) to (%f,%f), cell size=%f", getXMin(), getYMin(),
        getXMax(), getYMax(), getResolution());
  }

  /** @name API of the NearestNeighborsCapable virtual interface
    @{ */
  // See docs in base class
  [[nodiscard]] bool nn_has_indices_or_ids() const override { return false; }
  [[nodiscard]] size_t nn_index_count() const override { return 0; }
  [[nodiscard]] bool nn_single_search(
      const mrpt::math::TPoint3Df& query,
      mrpt::math::TPoint3Df& result,
      float& out_dist_sqr,
      uint64_t& resultIndexOrID) const override;
  [[nodiscard]] bool nn_single_search(
      const mrpt::math::TPoint2Df& query,
      mrpt::math::TPoint2Df& result,
      float& out_dist_sqr,
      uint64_t& resultIndexOrID) const override;
  void nn_multiple_search(
      const mrpt::math::TPoint3Df& query,
      const size_t N,
      std::vector<mrpt::math::TPoint3Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs) const override;
  void nn_multiple_search(
      const mrpt::math::TPoint2Df& query,
      const size_t N,
      std::vector<mrpt::math::TPoint2Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs) const override;
  void nn_radius_search(
      const mrpt::math::TPoint3Df& query,
      const float search_radius_sqr,
      std::vector<mrpt::math::TPoint3Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs,
      size_t maxPoints) const override;
  void nn_radius_search(
      const mrpt::math::TPoint2Df& query,
      const float search_radius_sqr,
      std::vector<mrpt::math::TPoint2Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs,
      size_t maxPoints) const override;
  /** @} */

 private:
  // See docs in base class
  double internal_computeObservationLikelihood(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const override;
  // See docs in base class
  bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const override;

  /** Returns a byte with the occupancy of the 8 sorrounding cells.
   * \param cx The cell index
   * \param cy The cell index
   * \sa direction2idx
   */
  inline unsigned char GetNeighborhood(int cx, int cy) const;

  /** Used to store the 8 possible movements from a cell to the
   *   sorrounding ones.Filled in the constructor.
   * \sa direction2idx
   */
  int direccion_vecino_x[8], direccion_vecino_y[8];

  /** Returns the index [0,7] of the given movement, or
   *  -1 if invalid one.
   * \sa direccion_vecino_x,direccion_vecino_y,GetNeighborhood
   */
  int direction2idx(int dx, int dy);

  MAP_DEFINITION_START(COccupancyGridMap2D)
  /** See COccupancyGridMap2D::COccupancyGridMap2D */
  float min_x{-10.0f}, max_x{10.0f}, min_y{-10.0f}, max_y{10.0f}, resolution{0.10f};
  /** Observations insertion options */
  mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts;
  /** Probabilistic observation likelihood options */
  mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts;
  MAP_DEFINITION_END(COccupancyGridMap2D)
};

bool operator<(
    const COccupancyGridMap2D::TPairLikelihoodIndex& e1,
    const COccupancyGridMap2D::TPairLikelihoodIndex& e2);
}  // namespace mrpt::maps
MRPT_ENUM_TYPE_BEGIN(mrpt::maps::COccupancyGridMap2D::TLikelihoodMethod)
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmMeanInformation);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmRayTracing);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmConsensus);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmCellsDifference);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmLikelihoodField_Thrun);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmLikelihoodField_II);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap2D, lmConsensusOWA);
MRPT_ENUM_TYPE_END()
